#include "lib_pid.h"


//增量式pid参数初始化
void incremental_pid_init(pid_set_t *incremental_pid_set_p)
{
	incremental_pid_set_p->Kp = 0;  //比例系数
	incremental_pid_set_p->Ki = 0;  //积分系数
	incremental_pid_set_p->Kd = 0;  //微分系数
	
	return ;
}

//增量式pid控制函数
float incremental_pid_control(u16_t *set_val,u16_t *get_val，pid_set_t *incremental_pid_set_p)
{
	float Error = 0;                 //当前偏差
	static float ErrorLast = 0;     //上次偏差值
	static float ErrorLastLast = 0; //上上次偏差值
    static float pid_out_old = 0; //上次pid输出值
	float pid_out = 0;            //当前pid输出值
	float pid_KPn = 0;             //比例项
	float pid_KIn = 0;            //积分项
	float pid_KDn = 0;            //微分项
	
	//1.计算当前偏差值
	Error = *set_val - *get_val;  
	//2.比例项计算
	pid_KPn = incremental_pid_set_p->Kp * (Error - ErrorLast); 
	//3.积分项计算
	pid_KIn = incremental_pid_set_p->Ki * Error; 
	//4.微分项计算
	pid_KDn = incremental_pid_set_p->Kd * (Error - 2 * ErrorLast + ErrorLastLast); 
	//5.记录上上次速度偏差值
	ErrorLastLast = ErrorLast;
	//6.记录上次速度偏差值
	ErrorLast = Error;  
	//7.当前pid输出值计算
	pid_out = pid_out_old + pid_KPn + pid_KIn + pid_KDn;
	//【可选】输出限幅处理
	pid_out = (pid_out > incre_pidOUTMAX ? incre_pidOUTMAX : pid_out);
	pid_out = (pid_out < incre_pidOUTMIN ? incre_pidOUTMIN : pid_out);
	//8.记录上次pid输出值
	pid_out_old = pid_out；
	
	//9.增量式pid输出值作用于执行器
	/*
	pid_out ==> Executor
	*/
	
	return pid_out;
}



